#include "drv_algorithm.h"
#include "math.h"

/**
 * @brief 角度制转弧度制
 */
float ALGO_Deg2Rad(const float deg)
{
    return deg / 180.0 * M_PI;
}

/**
 * @brief 弧度制转角度制
 */
float ALGO_Rad2Deg(const float rad)
{
    return rad / M_PI * 180.0;
}

/**
 * @brief 将角度限制到[-π, π]
 */
float ALGO_WrapToPi(const float rad)
{
    float wrap = fmodf(rad, 2 * M_PI);

    if (wrap < -M_PI)
    {
        wrap += 2 * M_PI;
    }
    else if (wrap > M_PI)
    {
        wrap -= 2 * M_PI;
    }

    return wrap;
}

/**
 * @brief 将角度限制到[-180°, 180°]
 */
float ALGO_WrapTo180(const float deg)
{
    float wrap = fmodf(deg, 2 * 180.0);

    if (wrap < -180.0)
    {
        wrap += 2 * 180.0;
    }
    else if (wrap > 180.0)
    {
        wrap -= 2 * 180.0;
    }

    return wrap;
}
